#include "tim.h"
#include "i2c.h"
#include "general_task_include.h"


/*---------------------------------------------------------------------------task state update-----------------------------------------------------------------------------------*/
static void StateUpdate(RobotState* robot_state, const NormRemoteCmd* norm_remote_cmd, NormRemoteCmd* norm_remote_cmd_last);
static void StateUpdateFromNewRec(RobotState* robot_state, const NormRemoteCmd* norm_remote_cmd, NormRemoteCmd* norm_remote_cmd_last);

RobotState robotState;
const RobotState* _robotState = &robotState;
/*记录上一次遥控信号，用于判断边缘*/
static NormRemoteCmd normRemoteCmdLast;



uint8_t readKeypadFromPCF8574(void);
void writeAndReadPCF8574();
uint8_t keypadState = 0x00;  // 默认没有按键被按下
uint8_t writeData =0xFF;
uint16_t keyVal;



/**
 * @brief 状态机任务，整车运动功能状态能且只能在这里进行修改操作
 */
void StateMachineTask(void* argument)
{	
	/*任务周期相关计算，并将其绑定到TaskMonitor相关指针中*/
	static uint32_t last_tick_count, current_tick_count, this_tick_count = 0;
	static uint16_t task_counter = 0;

	
	current_tick_count = last_tick_count = xTaskGetTickCount();	
	while(1)
	{		
		/*任务主进程*/
		
		//临时让其一直属于remote状态
		//StateUpdate(&robotState, _normRemoteCmd, &normRemoteCmdLast);
		robotState.ctrl_terminal = CONTROL_FROM_REMOTE;
		writeAndReadPCF8574();
	//	readKeypadFromPCF8574();
		/*任务状态更新*/
		task_counter++;
		current_tick_count = xTaskGetTickCount();
		this_tick_count = current_tick_count - last_tick_count;
		last_tick_count = current_tick_count;
		
		
		
		
		vTaskDelayUntil(&current_tick_count, STATE_TASK_PERIOD_SET);
	}
}
 





// 从 PCF8574 读取按键数据
uint8_t readKeypadFromPCF8574(void)
{
   
	keypadState = 0x00;
	
    // 从 PCF8574 读取一个字节
    HAL_I2C_Master_Receive(&hi2c2, PCF8574_READ_ADDRESS, &keypadState, 1, HAL_MAX_DELAY);
    
    return keypadState;  // 返回当前键盘状态
}



/*

	c0	c1	c2	c3

r0	1	2	3	4

r1	5	6	7	8

r2	9	10	11	12

r3	13	14	15	16


*/
void writeAndReadPCF8574()
{
	//扫描第0行
	 writeData=0xFE;
	 HAL_I2C_Master_Transmit(&hi2c2,PCF8574_WRITE_ADDRESS, &writeData,  1,  HAL_MAX_DELAY);
	 readKeypadFromPCF8574();
	 if(keypadState == writeData)
		 keyVal = keyVal;
	 else
	 {
		switch (keypadState)
		{
			case KEY0_0:	keyVal = 1;		break;
			case KEY0_1:	keyVal = 2;		break;
			case KEY0_2:	keyVal = 3;		break;
			case KEY0_3:	keyVal = 4;		break;
		}
	 }
	 
	//扫描第1行
	 writeData=0xFD;
	 HAL_I2C_Master_Transmit(&hi2c2,PCF8574_WRITE_ADDRESS, &writeData,  1,  HAL_MAX_DELAY);
	 readKeypadFromPCF8574();
	 	 if(keypadState == writeData)
		 keyVal = keyVal;
	 else
	 {
		switch (keypadState)
		{
			case KEY1_0:	keyVal = 5;		break;
			case KEY1_1:	keyVal = 6;		break;
			case KEY1_2:	keyVal = 7;		break;
			case KEY1_3:	keyVal = 8;		break;
		}
	 }
	 
	//扫描第2行
	 writeData=0xFB;
	 HAL_I2C_Master_Transmit(&hi2c2,PCF8574_WRITE_ADDRESS, &writeData,  1,  HAL_MAX_DELAY);
	 readKeypadFromPCF8574();
	 if(keypadState == writeData)
		 keyVal = keyVal;
	 else
	 {
		switch (keypadState)
		{
			case KEY2_0:	keyVal = 9;		break;
			case KEY2_1:	keyVal = 10;		break;
			case KEY2_2:	keyVal = 11;		break;
			case KEY2_3:	keyVal = 12;		break;
		}
	 }
	 
	//扫描第3行
	 writeData=0xF7;
	 HAL_I2C_Master_Transmit(&hi2c2,PCF8574_WRITE_ADDRESS, &writeData,  1,  HAL_MAX_DELAY);
	 readKeypadFromPCF8574();
	 if(keypadState == writeData)
		 keyVal = keyVal;
	 else
	 {
		switch (keypadState)
		{
			case KEY3_0:	keyVal = 13;		break;
			case KEY3_1:	keyVal = 14;		break;
			case KEY3_2:	keyVal = 15;		break;
			case KEY3_3:	keyVal = 16;		break;
		}
	 }

}

// 根据按键状态解析出具体按键
uint8_t decodeKey(uint8_t keypadState)
{
    for (int row = 0; row < 4; row++)
    {
        if ((keypadState & (1 << row)) == 0)  // 某行被按下
        {
            for (int col = 0; col < 4; col++)
            {
                if ((keypadState & (1 << (col + 4))) == 0)  // 某列被按下
                {
                    return row * 4 + col;  // 返回按键编号 (0-15)
                }
            }
        }
    }
    return 0xFF;  // 无按键按下
}
















/**
 * @brief 全局状态初始化，可用作控制中断时状态的刷新
 */
static void StateInit(RobotState* robot_state)
{
	robot_state->ctrl_terminal = CONTROL_STOP;
	robot_state->chassis_mode = CHASSIS_FOLLOW;
	robot_state->fric_mode = FRIC_OFF;
	robot_state->stir_mode = STIR_OFF;
	robot_state->auto_aim = robot_state->force_auto_aim = 0;
}

/**
 * @brief 全局状态更新（根据标准控制指令）
 */
static void StateUpdate(RobotState* robot_state, const NormRemoteCmd* norm_remote_cmd, NormRemoteCmd* norm_remote_cmd_last)
{
	switch(norm_remote_cmd->remote_source)
	{                     
		case ERROR_RECEIVE:
			StateInit(robot_state);
		break;
	
		case DT7:
			StateUpdateFromNewRec(robot_state, norm_remote_cmd, &normRemoteCmdLast);
		break;
	}
	memcpy(&normRemoteCmdLast, norm_remote_cmd, sizeof(NormRemoteCmd));
}

/**
 * @brief 根据新接收到的数据更新
 */
static void StateUpdateFromNewRec(RobotState* robot_state, const NormRemoteCmd* norm_remote_cmd, NormRemoteCmd* norm_remote_cmd_last)
{

	switch(norm_remote_cmd->Switch.switch_R1)
	{
		case NORM_RC_SW_UP:
			
				StateInit(robot_state);
			
		break;
		
		case NORM_RC_SW_MID:
			robot_state->ctrl_terminal = CONTROL_FROM_REMOTE;

	}
	


}